package fem2.element.crack_sda;

import fem2.MeshPart;
import fem2.Node;
import inf.jlinalg.BLAM;
import inf.jlinalg.IVector;

import java.util.ArrayList;

import math2.LinAlgUtilities;
import math2.MathUtilities;

public class SeparationMode {

	private IVector n;
	private double A;
	private double sA;
	private int[] posNodes;
	private ArrayList<Integer> negNodes = new ArrayList<Integer>();
	private MeshPart mp;

	public SeparationMode(MeshPart mp, IVector n, double A, int... posNodes) {
		this.mp = mp;
		this.n = n;
		this.A = A;
		this.posNodes = posNodes;
	}

	/**
	 * get the positive nodes
	 * 
	 * @return the posNodes
	 */
	public int[] getPositiveNodes() {
		return posNodes;
	}

	public boolean validate() {

		/*
		 * find all projection coordinate of nodes on direction n
		 */
		Node[] nodes = mp.getNodes();
		double[] s = new double[nodes.length];
		for (int i = 0; i < nodes.length; i++) {
			s[i] = project(nodes[i].getPosition());
		}

		/*
		 * 1-node separation
		 */
		if (posNodes.length == 1) {
			/*
			 * extract negative nodes
			 */
			for (int i = 0; i < 4; i++) {
				if (i != posNodes[0]) {
					negNodes.add(i);
				}
			}

			/*
			 * make sure the positive nodes have greatest projection coordinate
			 */
			double s1 = s[posNodes[0]];
			double s2 = Math.max(s[negNodes.get(0)],
					Math.max(s[negNodes.get(1)], s[negNodes.get(2)]));
			// assert s1 > s2 :
			// "the projection coordinate of positive node must be largest";
			if (s1 < s2) {
				// if (getEchoLevel().contain(EchoLevelType.DEBUG)) {
				// Debugger.watch("posNodes = ", posNodes);
				// Debugger.watch("negNodes = ", negNodes);
				// Debugger.watch("the projection coordinate of positive node must be largest");
				// }
				return false;
			}

			/*
			 * compute area of three sections in triangle section part
			 */
			double[] N1 = nodes[posNodes[0]].getPosition();
			double[] N2 = nodes[negNodes.get(0)].getPosition();
			double[] N3 = nodes[negNodes.get(1)].getPosition();
			double[] N4 = nodes[negNodes.get(2)].getPosition();
			double a1 = computeTriangleSectionArea1(s1, N1, N2, N3, N4);
			double a2 = computeTriangleSectionArea1((s1 + s2) / 2, N1, N2, N3, N4);
			double a3 = computeTriangleSectionArea1(s2, N1, N2, N3, N4);

			/*
			 * perform a parabola fit of (s1,a1), ((s1+s2)/2,a2), (s2,a3)
			 */
			double[] lhs = new double[] { Math.pow(s1, 2), s1, 1, Math.pow((s1 + s2) / 2, 2),
					(s1 + s2) / 2, 1, Math.pow(s2, 2), s2, 1 };
			double[] rhs = new double[] { a1, a2, a3 };
			LinAlgUtilities.backSolve(BLAM.NO_TRANSPOSE, lhs, rhs);

			/*
			 * find projection coordinate that create section of area A
			 */
			double[] sA = LinAlgUtilities.solveQuadratic(rhs[0], rhs[1], rhs[2] - A);
			if (sA == null) {
				return false;
			} else {
				if (sA.length == 1) {
					this.sA = sA[0];
				} else {
					if (sA[0] >= s2 && sA[0] <= s1) {
						this.sA = sA[0];
					} else {
						this.sA = sA[1];
					}
				}
				// if (getEchoLevel().contain(EchoLevelType.DEBUG)) {
				// Debugger.watch("a1 = ", a1);
				// Debugger.watch("a2 = ", a2);
				// Debugger.watch("a3 = ", a3);
				// Debugger.watch("A = ", A);
				// Debugger.watch("s1 = ", s1);
				// Debugger.watch("s2 = ", s2);
				// Debugger.watch("sA[0] = ", sA[0]);
				// Debugger.watch("sA[1] = ", sA[1]);
				// Debugger.watch("sA = ", this.sA);
				// Debugger.watch("error = "
				// + (computeTriangleSectionArea1(this.sA, N1, N2, N3, N4) -
				// A));
				// }
				// assert this.sA >= s2 && this.sA <= s1;
				if (!(this.sA >= s2 && this.sA <= s1)) {
					// if (getEchoLevel().contain(EchoLevelType.DEBUG)) {
					// Debugger.watch("the sA does not fall in valid range (s1,s2)");
					// }
					return false;
				} else {
					return true;
				}
			}
		}
		/*
		 * 2-node separation
		 */
		else if (posNodes.length == 2) {
			/*
			 * extract negative nodes
			 */
			for (int i = 0; i < 4; i++) {
				if (i != posNodes[0] && i != posNodes[1]) {
					negNodes.add(i);
				}
			}

			/*
			 * make sure the positive nodes have greater projection coordinate
			 */
			// assert s[posNodes[0]] > Math.max(s[negNodes.get(0)],
			// s[negNodes.get(1)]) :
			// "positive node should have larger projection coordinate";
			// assert s[posNodes[1]] > Math.max(s[negNodes.get(0)],
			// s[negNodes.get(1)]) :
			// "positive node should have larger projection coordinate";
			if (s[posNodes[0]] < Math.max(s[negNodes.get(0)], s[negNodes.get(1)])
					|| s[posNodes[1]] < Math.max(s[negNodes.get(0)], s[negNodes.get(1)])) {
				// if (getEchoLevel().contain(EchoLevelType.DEBUG)) {
				// Debugger.watch("the projection coordinate of positive nodes area is not valid");
				// }
				return false;
			}

			/*
			 * sort the projection coordinate
			 */
			// double s1;
			double s2;
			int n1, n2;
			if (s[negNodes.get(0)] < s[negNodes.get(1)]) {
				n1 = negNodes.get(0);
				n2 = negNodes.get(1);
			} else {
				n1 = negNodes.get(1);
				n2 = negNodes.get(0);
			}
			// s1 = s[n1];
			s2 = s[n2];

			double s3;
			// double s4;
			int n3, n4;
			if (s[posNodes[0]] < s[posNodes[1]]) {
				n3 = posNodes[0];
				n4 = posNodes[1];
			} else {
				n3 = posNodes[1];
				n4 = posNodes[0];
			}
			s3 = s[n3];
			// s4 = s[n4];

			/*
			 * modify positive nodes and negative nodes
			 */
			posNodes[0] = n3;
			posNodes[1] = n4;
			negNodes.set(0, n1);
			negNodes.set(1, n2);

			/*
			 * compute area of three sections on quadrilateral section part
			 */
			double[] N1 = nodes[n1].getPosition();
			double[] N2 = nodes[n2].getPosition();
			double[] N3 = nodes[n3].getPosition();
			double[] N4 = nodes[n4].getPosition();
			double a1 = computeQuadrilateralSectionArea(s2, N1, N2, N3, N4);
			double a2 = computeQuadrilateralSectionArea((s2 + s3) / 2, N1, N2, N3, N4);
			double a3 = computeQuadrilateralSectionArea(s3, N1, N2, N3, N4);

			/*
			 * perform a parabola fit of (s2,a1), ((s2+s3)/2,a2), (s3,a3)
			 */
			double[] lhs = new double[] { Math.pow(s2, 2), s2, 1, Math.pow((s2 + s3) / 2, 2),
					(s2 + s3) / 2, 1, Math.pow(s3, 2), s3, 1 };
			double[] rhs = new double[] { a1, a2, a3 };
			LinAlgUtilities.backSolve(BLAM.NO_TRANSPOSE, lhs, rhs);

			/*
			 * find projection coordinate that create section of area A
			 */
			double[] sA = LinAlgUtilities.solveQuadratic(rhs[0], rhs[1], rhs[2] - A);
			if (sA == null) {
				return false;
			} else {
				if (sA.length == 1) {
					this.sA = sA[0];
				} else {
					if (sA[0] >= s2 && sA[0] <= s3) {
						this.sA = sA[0];
					} else {
						this.sA = sA[1];
					}
				}
				// assert this.sA >= s2 && this.sA <= s3;
				if (!(this.sA >= s2 && this.sA <= s3)) {
					// if (getEchoLevel().contain(EchoLevelType.DEBUG)) {
					// Debugger.watch("the sA does not fall in valid range (s2,s3)");
					// }
					return false;
				} else {
					return true;
				}
			}
		} else {
			throw new Error("number of positive nodes can't exceed 2");
		}
	}

	/**
	 * get the crack normal associated with separation mode
	 * 
	 * @return the n
	 */
	public IVector getSeparationNormal() {
		return n;
	}

	/**
	 * get crack area associated with separation mode
	 * 
	 * @return the a
	 */
	public double getCrackArea() {
		return A;
	}

	/**
	 * get the crack boundary of this separation mode
	 * 
	 * @return
	 */
	public ArrayList<double[]> getCrackBoundary() {
		ArrayList<double[]> bound = new ArrayList<double[]>();
		Node[] nodes = mp.getNodes();
		if (posNodes.length == 1) {
			double[] N1 = nodes[posNodes[0]].getPosition();
			double[] N2 = nodes[negNodes.get(0)].getPosition();
			double[] N3 = nodes[negNodes.get(1)].getPosition();
			double[] N4 = nodes[negNodes.get(2)].getPosition();
			bound.add(computeIntersection(sA, N1, N2));
			bound.add(computeIntersection(sA, N1, N3));
			bound.add(computeIntersection(sA, N1, N4));
		} else if (posNodes.length == 2) {
			int n1 = negNodes.get(0);
			int n2 = negNodes.get(1);
			int n3 = posNodes[0];
			int n4 = posNodes[1];
			double[] N1 = nodes[n1].getPosition();
			double[] N2 = nodes[n2].getPosition();
			double[] N3 = nodes[n3].getPosition();
			double[] N4 = nodes[n4].getPosition();
			bound.add(computeIntersection(sA, N1, N3));
			bound.add(computeIntersection(sA, N2, N3));
			bound.add(computeIntersection(sA, N2, N4));
			bound.add(computeIntersection(sA, N1, N4));
		}
		return bound;
	}

	private double project(double[] X) {
		double p = BLAM.norm(BLAM.NORM_TWO, n);
		return (X[0] * n.get(0) + X[1] * n.get(1) + X[2] * n.get(2)) / Math.pow(p, 2);
	}

	private double[] computeIntersection(double s, double[] X1, double[] X2) {
		double d = -s * Math.pow(BLAM.norm(BLAM.NORM_TWO, n), 2);
		return MathUtilities.computeIntersection(n.get(0), n.get(1), n.get(2), d, X1[0], X1[1],
				X1[2], X2[0] - X1[0], X2[1] - X1[1], X2[2] - X1[2]);
	}

	private double computeTriangleSectionArea1(double s, double[] X1, double[] X2, double[] X3,
			double[] X4) {
		double[] x1 = computeIntersection(s, X1, X2);
		double[] x2 = computeIntersection(s, X1, X3);
		double[] x3 = computeIntersection(s, X1, X4);
		return MathUtilities.heron(x1, x2, x3);
	}

	private double computeQuadrilateralSectionArea(double s, double[] X1, double[] X2, double[] X3,
			double[] X4) {
		double[] x1 = computeIntersection(s, X1, X3);
		double[] x2 = computeIntersection(s, X2, X3);
		double[] x3 = computeIntersection(s, X2, X4);
		double[] x4 = computeIntersection(s, X1, X4);
		return MathUtilities.heron(x1, x2, x3) + MathUtilities.heron(x1, x3, x4);
	}

	private double computeTriangleSectionArea2(double s, double[] X1, double[] X2, double[] X3,
			double[] X4) {
		double[] x1 = computeIntersection(s, X3, X4);
		double[] x2 = computeIntersection(s, X2, X4);
		double[] x3 = computeIntersection(s, X1, X4);
		return MathUtilities.heron(x1, x2, x3);
	}

	public ArrayList<double[]> getCrackBoundaryByScanning() {

		/*
		 * scan all the projection coordinate of all the nodes
		 */
		Node[] nodes = mp.getNodes();
		double[] s = new double[nodes.length];
		for (int i = 0; i < nodes.length; i++) {
			s[i] = project(nodes[i].getPosition());
		}

		/*
		 * sort all the projection coordinate in descending order
		 */
		int[] ids = MathUtilities.sort(s, true);

		double s1 = s[ids[0]];
		double s2 = s[ids[1]];
		double s3 = s[ids[2]];
		double s4 = s[ids[3]];
		double[] N1 = nodes[ids[0]].getPosition();
		double[] N2 = nodes[ids[1]].getPosition();
		double[] N3 = nodes[ids[2]].getPosition();
		double[] N4 = nodes[ids[3]].getPosition();

		double smax = 0;
		ArrayList<double[]> bound = new ArrayList<double[]>();

		/*
		 * scan the first triangle section
		 */
		if (s1 != s2) {
			/*
			 * search for the projection coordinate which match the crack area
			 */
			double a1 = computeTriangleSectionArea1(s1, N1, N2, N3, N4);
			double a2 = computeTriangleSectionArea1((s1 + s2) / 2, N1, N2, N3, N4);
			double a3 = computeTriangleSectionArea1(s2, N1, N2, N3, N4);
			/*
			 * perform a parabola fit of (s1,a1), ((s1+s2)/2,a2), (s2,a3)
			 */
			double[] lhs = new double[] { Math.pow(s1, 2), s1, 1, Math.pow((s1 + s2) / 2, 2),
					(s1 + s2) / 2, 1, Math.pow(s2, 2), s2, 1 };
			double[] rhs = new double[] { a1, a2, a3 };
			LinAlgUtilities.backSolve(BLAM.NO_TRANSPOSE, lhs, rhs);

			/*
			 * find projection coordinate that create section of area A
			 */
			double[] sA = LinAlgUtilities.solveQuadratic(rhs[0], rhs[1], rhs[2] - A);

			if (sA != null) {
				if (sA.length == 1) {
					this.sA = sA[0];
				} else {
					if (sA[0] >= s2 && sA[0] <= s1) {
						this.sA = sA[0];
					} else {
						this.sA = sA[1];
					}
				}
			}

			// Debugger.watch("S1 = ", computeTriangleSectionArea1(s1, N1,
			// N2, N3, N4));
			// Debugger.watch("S2 = ", computeTriangleSectionArea1(s2, N1,
			// N2, N3, N4));

			if (this.sA >= s2 && this.sA <= s1) {
				bound.add(computeIntersection(this.sA, N1, N2));
				bound.add(computeIntersection(this.sA, N1, N3));
				bound.add(computeIntersection(this.sA, N1, N4));
				return bound;
			}

		}

		/*
		 * scan the second quadrilateral section
		 */
		if (s2 != s3) {
			/*
			 * search for the projection coordinate which match the crack area
			 */
			double a1 = computeQuadrilateralSectionArea(s2, N1, N2, N3, N4);
			double a2 = computeQuadrilateralSectionArea((s2 + s3) / 2, N1, N2, N3, N4);
			double a3 = computeQuadrilateralSectionArea(s3, N1, N2, N3, N4);
			/*
			 * perform a parabola fit of (s2,a1), ((s2+s3)/2,a2), (s3,a3)
			 */
			double[] lhs = new double[] { Math.pow(s2, 2), s2, 1, Math.pow((s2 + s3) / 2, 2),
					(s2 + s3) / 2, 1, Math.pow(s3, 2), s3, 1 };
			double[] rhs = new double[] { a1, a2, a3 };
			LinAlgUtilities.backSolve(BLAM.NO_TRANSPOSE, lhs, rhs);

			/*
			 * find projection coordinate that create section of area A
			 */
			double[] sA = LinAlgUtilities.solveQuadratic(rhs[0], rhs[1], rhs[2] - A);

			if (sA != null) {
				if (sA.length == 1) {
					this.sA = sA[0];
				} else {
					if (sA[0] >= s3 && sA[0] <= s2) {
						this.sA = sA[0];
					} else {
						this.sA = sA[1];
					}
				}
			}

			// Debugger.watch("S2 = ", computeQuadrilateralSectionArea(s2,
			// N1, N2, N3, N4));
			// Debugger.watch("Smax = ",
			// computeQuadrilateralSectionArea(-rhs[1] / (2 * rhs[0]), N1,
			// N2, N3, N4));
			// Debugger.watch("S3 = ", computeQuadrilateralSectionArea(s3,
			// N1, N2, N3, N4));

			smax = -rhs[1] / (2 * rhs[0]);

			if (this.sA >= s3 && this.sA <= s2) {
				bound.add(computeIntersection(this.sA, N1, N3));
				bound.add(computeIntersection(this.sA, N2, N3));
				bound.add(computeIntersection(this.sA, N2, N4));
				bound.add(computeIntersection(this.sA, N1, N4));
				return bound;
			}

		}

		/*
		 * scan the third triangle section
		 */
		if (s3 != s4) {
			/*
			 * search for the projection coordinate which match the crack area
			 */
			double a1 = computeTriangleSectionArea2(s3, N1, N2, N3, N4);
			double a2 = computeTriangleSectionArea2((s3 + s4) / 2, N1, N2, N3, N4);
			double a3 = computeTriangleSectionArea2(s4, N1, N2, N3, N4);
			/*
			 * perform a parabola fit of (s3,a1), ((s3+s4)/2,a2), (s4,a3)
			 */
			double[] lhs = new double[] { Math.pow(s3, 2), s3, 1, Math.pow((s3 + s4) / 2, 2),
					(s3 + s4) / 2, 1, Math.pow(s4, 2), s4, 1 };
			double[] rhs = new double[] { a1, a2, a3 };
			LinAlgUtilities.backSolve(BLAM.NO_TRANSPOSE, lhs, rhs);

			/*
			 * find projection coordinate that create section of area A
			 */
			double[] sA = LinAlgUtilities.solveQuadratic(rhs[0], rhs[1], rhs[2] - A);

			if (sA != null) {
				if (sA.length == 1) {
					this.sA = sA[0];
				} else {
					if (sA[0] >= s4 && sA[0] <= s3) {
						this.sA = sA[0];
					} else {
						this.sA = sA[1];
					}
				}
			}

			// Debugger.watch("S3 = ", computeTriangleSectionArea2(s3, N1,
			// N2, N3, N4));
			// Debugger.watch("S4 = ", computeTriangleSectionArea2(s4, N1,
			// N2, N3, N4));

			if (this.sA >= s4 && this.sA <= s3) {
				bound.add(computeIntersection(this.sA, N1, N4));
				bound.add(computeIntersection(this.sA, N2, N4));
				bound.add(computeIntersection(this.sA, N3, N4));
				return bound;
			}

		}

		// for (int i = 0; i < nodes.length; i++) {
		// Debugger.watch(nodes[i].getPosition());
		// }
		// Debugger.watch("A = ", A);
		// Debugger.watch("normal = ", n);
		// Debugger.watch("posNodes = ", posNodes);

		/*
		 * choose the middle section for crack surface as a work around
		 */
		bound.add(computeIntersection(smax, N1, N3));
		bound.add(computeIntersection(smax, N2, N3));
		bound.add(computeIntersection(smax, N2, N4));
		bound.add(computeIntersection(smax, N1, N4));
		return bound;

		// throw new Error("could not find the matching crack area");
	}
}